#include <webots/DistanceSensor.hpp>
#include <webots/Motor.hpp>
#include <webots/Robot.hpp>
#include "wheel_control.hpp"
#include <string>

/*usb通信代码版本选择*/
#define USB_CPP 1
#define USB_C 2
#define USB_MODE USB_CPP

#if USB_MODE == USB_C
extern "C"
{
#include "usb_com.h"
}
#elif USB_MODE == USB_CPP
#include "usb_com.hpp"
#endif // USB_CPP

#define TIME_STEP 64
using namespace webots;

int main(int argc, char **argv)
{
  Robot robot;
  const char *wheel_name[] = {"lf_wheel_1", "rf_wheel_2", "lr_wheel_3", "rr_wheel_4"};
  wheel_ctl_t wheel_crl(&robot, wheel_name);

/*================Open Serial=====================*/
#if USB_MODE == USB_C
  const char *com = "COM2";
  HANDLE comHandle = INVALID_HANDLE_VALUE;
  comHandle = OpenSerial(com, CBR_9600, 8, NOPARITY, ONESTOPBIT);
  if (INVALID_HANDLE_VALUE == comHandle)
  {
    LOGE("OpenSerial COM2 fail!\r\n");
    return -1;
  }
  else
  {
    LOGD("[auv_ctrl]Open COM2 Successfully!\r\n");
  }
#elif USB_MODE == USB_CPP
#ifdef CE_WINDOWS
  ceSerial com(ceSerial::genComNumber(1), 115200, 8, 'N', 1);
#else
  ceSerial com("/dev/ttyS0", 9600, 8, 'N', 1); // Linux
#endif
  // 输出开启汇报
  printf("Opening port %s.\n", com.GetPort().c_str());
  if (com.Open() == 0)
  {
    printf("OK.\n");
  }
  else
  {
    printf("Error.\n");
    return 1;
  }
  // 发送测试数据
  bool successFlag;
  printf("Writing.\n");
  char s[] = "Hello";
  successFlag = com.Write(s);       // write string
  successFlag = com.WriteChar('!'); // write a character

  char rec_char = 0;
  char send_str[] = "";
#endif // USB_MODE

  while (robot.step(TIME_STEP) != -1) ///////////////////////////////开启循环
  {
/*=============Receive Data==================*/
#if USB_MODE == USB_C
    if (ReadSerial(comHandle) == FALSE)
    {
      break;
    }
#elif USB_MODE == USB_CPP
    rec_char = com.ReadChar(successFlag); // read a char
    if (successFlag)
    {
      printf("Rx: %c\n", rec_char);
      sprintf(send_str, "[RES]Receive %c\r\n", rec_char);
      com.Write(send_str);
      switch (rec_char)
      {
      case 'w':
        wheel_crl.go(1.0);
        break;

      case 'a':
        wheel_crl.left(1.0);
        break;

      case 'd':
        wheel_crl.right(1.0);
        break;

      case 's':
        wheel_crl.back(1.0);
        break;

      case '.':
        wheel_crl.stop();
        break;

      default: //
        break; //
      }
    }

#endif // USB_MODE

  } ///////////////////////////////////////////////////////////退出循环
  // delete robot;
  return 0; // EXIT_SUCCESS
}
